

// PCL
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/common/transforms.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/approximate_voxel_grid.h>
#include <pcl/registration/gicp.h>

#include <geometry_utils/Transform3.h>
#include <sstream>
#include <fstream>
#include <iostream>
#include <string>
#include "time_counter.hpp"
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <point_new.h>
using namespace std;

Eigen::Affine3f transform0;

std::string itos(int i)
{
    std::stringstream s;
    s << i;
    return s.str();
}

template <typename Point>
void pubpc(ros::Publisher &pub, pcl::PointCloud<Point> &pc, std::string frame = "base_link")
{
    sensor_msgs::PointCloud2 msgs;
    pcl::toROSMsg(pc, msgs);
    msgs.header.frame_id = frame;
    pub.publish(msgs);
}

int i = 0;
const int jump = 1;
void velodyne_callback(const sensor_msgs::PointCloud2ConstPtr &cloud)
{
    if ((i++) % jump != 0)
        return;
    CloudPtr frames(new Cloud);
    pcl::fromROSMsg(*cloud, *frames);

    pcl::transformPointCloud(*frames, *frames, transform0);


//    // 生成pcd
//    pcl::PCDWriter writer;
//    cout << "writing NO: " << i / jump << endl;
//    std::string name = std::to_string(i / jump + 202300000);
//    writer.write("/media/zwh/T7/ZWH/bags/2023/zrh/3lidae3camera/" + name + ".pcd", *frames);


//    if ((i++) % jump != 0)
//        return;
//    pcl::PointCloud<pcl::PointXYZI>::Ptr frames(new pcl::PointCloud<pcl::PointXYZI>);
//    pcl::fromROSMsg(*cloud, *frames);
//
//    pcl::transformPointCloud(*frames, *frames, transform0);
//
//    // 生成pcd
//    pcl::PCDWriter writer;
//    cout << "writing NO: " << i / jump << endl;
//    std::string name = std::to_string(i / jump + 202300000);
//    writer.write("/home/zwh/work_space/18xx/calibration-tool/TEST_DATA/nanchuan/hw3_2_c79/" + name + ".pcd", *frames);



    // writer.write("/home/zwh/work_space/18xx/calibration-tool/TEST_DATA/vlp16/" + name + ".pcd", *frames);

    // cout << "before down:" << frames->size() << endl;
    // pcl::VoxelGrid<pcl::PointXYZI> sor;
    // sor.setInputCloud(frames);
    // sor.setLeafSize(2, 2, 1.5);
    // sor.filter(*frames);
    // cout << "after down:" << frames->size() << endl;
    // ofstream OutFile("/media/zwh/T7/ZWH/bags/dataset/changan/pika0/test.obj"); //利用构造函数创建obj文本
    // for (int i = 0; i < frames->size(); i++)
    // {
    //     pcl::PointXYZI &pt = frames->points[i];
    //     if (pt.x < 100 && pt.x > 1 && pt.y < 20 && pt.y > -20 && pt.z < 5 && pt.z > -5)
    //         OutFile << "v " << frames->points[i].x << " " << frames->points[i].y << " " << frames->points[i].z << endl;
    //     // if(i>0)
    //     // OutFile << "l " << i << " " << i+1 << endl;
    // }
    // OutFile.close();

     // 生成bin
     std::string out_file = "/home/zwh/temp/" + to_string(23020500000 + i / jump) + ".bin";
     std::ofstream bin_file(out_file.c_str(), std::ios::out | std::ios::binary);

     for (int j = 0; j < frames->size(); j++)
     {

         bin_file.write((char *)&frames->at(j).x, sizeof(frames->at(j).x));
         bin_file.write((char *)&frames->at(j).y, sizeof(frames->at(j).y));
         bin_file.write((char *)&frames->at(j).z, sizeof(frames->at(j).z));
         bin_file.write((char *)&frames->at(j).intensity, sizeof(frames->at(j).intensity));
     }
     bin_file.close();

     std::cout << "生成文件:" << out_file << std::endl;
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "bag2pcd");
    ros::NodeHandle nh;

    ros::Subscriber sub = nh.subscribe("/pointcloud_lidar3", 10, velodyne_callback);

    //注意是否转坐标
    transform0 = Eigen::Affine3f::Identity();
//    2.6, 0.0, 0.0, 0.0, 0.01, 0.0
// 0.88252,0.85335,0.0556839,-0.0139905,-0.0157,1.57237
// 0.684717,-0.270158,0.00258499,-3.13517,-3.14022,1.55372
//     transform0.translation() << 0.684717,-0.270158,0.00258499;
//     transform0.rotate(Eigen::AngleAxisf(1.55372, Eigen::Vector3f::UnitZ()));
//     transform0.rotate(Eigen::AngleAxisf(-3.14022, Eigen::Vector3f::UnitY()));
//     transform0.rotate(Eigen::AngleAxisf(-3.13517, Eigen::Vector3f::UnitX()));

    ros::spin();

    return 0;
}
